#note: the range scanner's degree is +90
#note: added  3.times{temprange.shift} to drawranges
#can only be used with type lines

require "gl"
require "glu"
require "glut"
require "mathn"

#Add gl and glut namespaces in order to make porting easier
include Gl
include Glu
include Glut


CONFIGFILENAME="geomapper2D.cfg"
 
#default variables
$scale=3.0 #default value,normally drawn from the config file
$robotno=1
$viewcount=1
$logfilename="../../mapdata/geomap_robot#{$viewcount}.dat" #default log file name value
$type="dots" 
$delay=0.01 #sets the delay refresh rate of the Mapper to increase performance (0.2-0.3)

#Program variables
$start_x=0
$start_y=0
$inslogarray=[]
$fsonar=[]
$rsonar=[] #Just added 2008/11/10
$robotrange=[]

$fsonarpos=[-90,-50,-30,-10,10,30,50,90] #the position of the fsonar on the P2AT Bot
$rsonarpos=[90,130,150,170,-170,-150,-130,-90] #the position of the rsonar on the P2AT Bot

$fsonarpara=+0.1
$fsonarypos=[0.145,0.185+$fsonarpara,0.22+$fsonarpara,0.24+$fsonarpara,0.24+$fsonarpara,0.22+$fsonarpara,0.185+$fsonarpara,0.145-0.1]
$fsonarxpos=[-0.130,-0.115+$fsonarpara,-0.080+$fsonarpara,-0.025+$fsonarpara,0.025+$fsonarpara,0.080+$fsonarpara,0.015+$fsonarpara,0.130-0.1]

$rsonarxpos=[-0.145,-0.185,-0.22,-0.24,-0.24,-0.22,-0.185,-0.145]
$rsonarypos=[0.130,0.115,0.080,0.025,-0.025,-0.080,-0.015,-0.130]
window =""
$screen=1


#OpenGl Variable 
$viewx=0 #view point starting view, default=0
$viewy=0 #view point starting view,default=0
$s_width=640
$s_height=480
$look_x=0.0
$look_y=0.0

#draw which element
$drawrs=0
$drawfs=1
$laserscan=1
$showlaserray=0

#used for multiple robots
$robotname=[]

#cumilative data
$mem_robotpos=[]
$mem_start_x=[]
$mem_start_y=[]
$mem_robotname=[]
$mem_robotins=[]

#opengl data
$mem_wall=[] # $mem_wall[robotno][[x,y],[x,y]....]


#2D View simulated by drawing only on the -6.0 z-plane and viewing only upwards
#from the -12.0 z-plane

#get the starting xy value of the robot (UNUSED)
def getstartxy()
 open($logfilename,"r"){|file|
   file.each_line { |line|  
  if (line =~ /INS,/)
  temparray=line.split(",") #reads the first line
  $start_x=temparray[2].to_f
  $start_y=temparray[3].to_f
  break 
  end
  }
 }
end

#This function reads the configuration file 
def readconfig()
  
   #try to read the config file (mapper.cfg)
   raise "Cannot read config file #{CONFIGFILENAME}"  unless (File.file?(CONFIGFILENAME))
   open(CONFIGFILENAME,"r"){|file|
   raise "config file corrupted" unless (file.readline()=~ /\[USARSIM_LIVE_MAPPER\]/)
   
   #load the config file data
    file.each_line() { |line|      

      #set the map scale from the config file 
      if line =~ /\[MAP_SCALE=([A-Za-z.0-9]+)\]/
        mapscale=$1 
        if mapscale =~/[0-9]*\.[0-9]+|[0-9]+/  and mapscale.to_f <100 and mapscale.to_f >0
          $scale=mapscale.to_f
          puts "setting map scale data to #{$scale}"
        else
         puts "cannot read the map scale parameter, defaulting to #{$scale}, should be set to something like MAP_SCALE=3.0"
       end
      end
      #set the map type from the config file
       if line =~ /\[TYPE=([A-Za-z.0-9]+)\]/
         maptype=$1
        if maptype =~/lines|dots|line|dot/i #check if the type is lines,dots 
         $type=maptype 
          puts "setting map type to #{$type}"
        else
         puts "cannot read the map type parameter, defaulting to #{$type}, options include lines or dots"
       end
      end
      #set the update delay from the config file
         if line =~ /\[UPDATE_DELAY=([A-Za-z.0-9]+)\]/
         upde=$1
        if upde =~ /[0-9]*\.[0-9]+|[0-9]+/ #check if the number is a float
         $delay=upde.to_f
          puts "setting the update delay speed to #{$delay} seconds"
        else
         puts "cannot read the update delay parameter, defaulting to #{$type}, should be set to (0.1-0.3)"
       end
      end
      
      
    }
   } 
 
end

#This function deals with the opengl windows settings
def init_gl_window(width ,height )

#glClearColor specifies the red, green, blue, and alpha values used by
#glClear to clear the color buffers. sets the value to be used in glClear(GL_COLOR_BUFFER_BIT)
glClearColor(0.0,0.0,0.0,0) 

glClearDepth(1.0) # enables the clearing of the depth buffer, used in glClear(GL_DEPTH_BUFFER_BIT)
glDepthFunc(GL_LEQUAL) #set type of depth
glEnable(GL_DEPTH_TEST) #enables depth test
glShadeModel(GL_SMOOTH) # smooth color shading

glMatrixMode(GL_PROJECTION) #Applies subsequent matrix operations to the projection matrix stack.
glLoadIdentity #clear old matrix
gluPerspective(45.0, width / height, 0.1, 100.0)# Calculate aspect ratio of the window 45 degrees,ratio of width/height, Znear,Zfar.
glMatrixMode(GL_MODELVIEW) #Applies subsequent matrix operations to the modelview matrix stack.
draw_gl_scene #draw scence
end

#This function deals with what happends when the windows reshapes itself
def reshape(width, height)
    height = 1 if height == 0 #prevent division by zero
    # Reset current viewpoint and perspective transformation
    # glViewport(Lowerright_x,Lowerright_y,width,height)
    $s_width=width
    $s_height=height
    glViewport($viewx, $viewy, width, height)

    glMatrixMode(GL_PROJECTION) #Applies subsequent matrix operations to the projection matrix stack.
    glLoadIdentity #Clear the Projection matrix stack 
    gluPerspective(45.0, $s_width / $s_height, 0.1, 100.0) #reset the  Perspective
end

#inputs a degree in and return the value of it's radian
def de2rad(float)
  rad=float
  pi=3.14159265
  if (rad>0)
    return ((2*pi)/360)*rad
  else
    return 2*pi+(((2*pi)/360)*rad)
  end
end

#draws a wallfrom the radian,distance and the starting x,y
def drawwall(radian,distance,startx,starty,robotface)
  rad=radian.to_f
  dis=distance.to_f
  sx=startx.to_f
  sy=starty.to_f
  robotang=robotface.to_f
  #don't draw rays that have a distance of more than 4.9 (it might be the max range of the sensor)
  unless(dis>4.9)
  drawx=(dis)*(Math.cos(rad+robotang))
  drawy=(dis)*(Math.sin(rad+robotang))
    glBegin(GL_POINTS)
      #glVertex3f( (sx/$scale)+(drawx/$scale) ,(sy/$scale)+(drawy/$scale),0.0 )
      glVertex3f( (sx+drawx)/$scale ,(sy+drawy)/$scale ,0.0 )
    glEnd
  end   
end

#draws a wallfrom the radian,distance and the starting x,y
def drawwallrange(radian,distance,startx,starty,robotface)
  rad=radian.to_f
  dis=distance.to_f
  sx=startx.to_f
  sy=starty.to_f
  robotang=robotface.to_f
  #don't draw rays that have a distance of more than 20.0 (it might be the max range of the sensor)
  if(dis<19.9)
  drawx=(dis)*(Math.cos(rad+robotang))
  drawy= (dis)*(Math.sin(rad+robotang))
    glBegin(GL_POINTS)
      #glVertex3f( (sx/$scale)+(drawx/$scale) ,(sy/$scale)+(drawy/$scale),0.0 )
      glVertex3f( (sx+drawx)/$scale ,(sy+drawy)/$scale ,0.0 )
    glEnd
  end   
end

def drawrsonar(insarray,rsonararray)
  ins=insarray 
  rsonar=rsonararray
  

if($drawrs==1)
  #lookat the instructions from the drawrsonar function
  rsonar.each_with_index { |line,index| 
    temprsonar=line.split(",")
    if(temprsonar[1].to_f>0) #ignore if time <0 
    #for each sonar position
    tempins=ins[index-1].split(",")  #get the xyz position 
    robotx=tempins[3].to_f-0#startx
    roboty=tempins[4].to_f-0#starty
 
  #for each sonar 
  $rsonarpos.each_with_index { |pos,index| 
    drawwall(de2rad(pos),temprsonar[index+3],robotx,roboty,tempins[8].to_f) #noposition
    # drawwall(de2rad(pos),temprsonar[index+2],robotx+($rsonarxpos[index]*2),roboty+($rsonarypos[index]*2),tempins[7].to_f) #with position
      } 
     end #end time  
    }
  end
end
  
def drawrangescan(insarray,rangescanarray)
  ins=insarray
  range=rangescanarray

if($laserscan==1)
  range.each_with_index { |line,index| 
    temprange=line.split(",")
    if(temprange[1].to_f>0) #ignore if time <0 
    3.times{temprange.shift}

    #for each sonar position
    tempins=ins[index-1].split(",")  #get the xyz position 
    robotx=tempins[3].to_f-0#(startx)
    roboty=tempins[4].to_f-0#(starty)

    #for each range
    temprange.each_with_index { |member,index| 
    drawwallrange((0.0174*(temprange.size-index)),member.to_f,robotx,roboty,tempins[8].to_f-(3.1415/2) ) # no position
      } 
    end #end time 
  }
end
end

def drawfsonar(insarray,fsonararray)
  ins=insarray 
  fsonar=fsonararray
  #note, the insarray and fsonararray always come together so insarray[1] has the position of fsonararray[1]
  #the fsonar position (degrees) for the P2AT is -90,-50,-30,-10,10,30,50,90
  #sample fsoanr line is FSONAR,463.45,1.5091,4.9983,5.0000,4.9953,4.9963,4.3348,3.3470,4.8052

if($drawfs==1)
  fsonar.each_with_index { |line,index| 
  tempfsonar=line.split(",")
  if(tempfsonar[1].to_f>0) #ignore if time <0 
  #for each sonar position
  tempins=ins[index-1].split(",")  #get the xyz position 
  robotx=tempins[3].to_f-0#(startx)
  roboty=tempins[4].to_f-0#(starty)
  #for each sonar 
  $fsonarpos.each_with_index { |pos,index| 
   # $fsonarpos=[-90,-50,-30,-10,10,30,50,90] #the position of the fsonar on the P2AT Bot
   if(tempins[8].to_f!=0)
   glColor(0.0,1.0, 1.0)
  # drawwall(de2rad(pos),tempfsonar[index+3].to_f,robotx+$fsonarxpos[index]/$scale,roboty+$fsonarypos[index]/$scale,tempins[8].to_f) #with position
   # glColor(1.0,0.0, 1.0)
   drawwall(de2rad(pos),tempfsonar[index+3],robotx,roboty,tempins[8].to_f) # no position
   end
    } 
  end #end time   
  }
     
 end 
end

#This function draws the INS Position
def drawINS(array,startx,starty) 
 insarray=array
 lines_old_x=startx/$scale
 lines_old_y=starty/$scale
 insarray.each_with_index { |line,index| 
  temparray=line
    #draws lines 
     if ($type=~/lines|line/i and index>0)
    glBegin(GL_LINES)
      glVertex3f(lines_old_x,lines_old_y,0.0)
      glVertex3f( (temparray[3].to_f)/$scale , (temparray[4].to_f)/$scale,0.0 )
    # puts "Hi: #{temparray[3].to_f-startx}"
    glEnd
    #store the points on a variable
    lines_old_x=(temparray[3].to_f)/$scale
    lines_old_y=(temparray[4].to_f)/$scale
    end
  }
end


#output a string of text to be shown on open_gl
def output(fx,fy,fz,fstring)
  x=fx
  y=fy
  z=fz
  ostring=fstring
  i=0
  glRasterPos3f(x, y,z);
  ostring.length.times{|i|
    glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18,ostring[i] );
  }
end


def draw_map()
  
  #draw a yellow X at the start point  
  glPushMatrix()
  glColor(1.0, 1.0, 0.0)
  glTranslatef(0.05,0.0,0.0) #move the letter x to the starting poinst
  #Start center of the screen with a depth of six 
  #output(0.0,-0.03,-6.0,"x")  #default lenght of the character X
 $mem_start_x.length.times{|i| 
  output($mem_start_x[i]/$scale, ($mem_start_y[i]/$scale)-0.03 ,-6.0,"x")
 }
  glPopMatrix()
 
#draw last robot position
$mem_robotpos.length.times{|i|
  glPushMatrix()
  glColor(1.0, 0.0, 0.0)
  temparray=Array.new($mem_robotpos[i])
  # get the robot position 
  robotx=temparray[3]
  roboty=temparray[4]
  
  glTranslatef( (robotx/$scale),(roboty/$scale),-6.0)  
  quadratic=gluNewQuadric();
  gluSphere(quadratic,0.05,32,32)
  glPopMatrix()
  
  #draw robot head Note INS Pitch,Roll,Yaw data is in radians when the yaw isn't zero
 if(temparray[8].to_f!=0)
  glPushMatrix()
  glColor(1.0,1.0,1.0)
  #calculate the head angle
  headx=0.05*(Math.cos(temparray[8].to_f))
  heady=0.05*(Math.sin(temparray[8].to_f))
  #puts "headx=#{headx} heady=#{heady} robotx=#{robotx} roboty=#{roboty}"
  #glTranslatef( ((robotx/$scale)+headx),((roboty/$scale)+heady)*-1,-6.0) #inverted mode
   glTranslatef( ((robotx/$scale)+headx),((roboty/$scale)+heady),-6.0)   
  quadratic=gluNewQuadric();
  gluSphere(quadratic,0.025,32,32)
  glPopMatrix()
 end 
}

# draw robot range 
  $mem_robotins.length.times{|i| 
  glPushMatrix()
  glColor(1.0/(i+1), 1.0/(i+i), 1.0/(i+i))
  glTranslatef(0.0, 0.0, -6.0)   #Start center of the screen with a depth of six
  drawINS($mem_robotins[i],$mem_start_x[i],$mem_start_y[i])   
  glPopMatrix()
  }
  
# draws the wall
  glPushMatrix()
  glTranslatef(0.0, 0.0, -6.0)
  #puts $mem_wall[0].size if $mem_wall[0]
$mem_wall.each{|robot|
    robot.each{|wall|
    glBegin(GL_POINTS)
     glColor(1.0, 1.0,1.0)
      glVertex3f( wall[0]/$scale ,wall[1]/$scale,wall[2]/$scale)
    glEnd
    }
}  
glPopMatrix()
  
# UNCOMMENT HERE 
#  #draw the sonar rays from the robot  
#  $robotname.length.times{|i|
#  glPushMatrix()
#  glColor(0.0, 1.0/(i+1), 1.0/(2*i+1))
#  glTranslatef(0.0, 0.0, -6.0)   #Start center of the screen with a depth of six
#  #glRotate(180,1,0,0) 
#  finalpos=$robotinsarray[i][$robotinsarray[i].length-1].split(",") 
#  finalfson=$robotfsonar[i][$robotfsonar[i].length-1].split(",") 
#  finalrson=$robotrsonar[i][$robotrsonar[i].length-1].split(",")
#  finalrange=$robotrange[i][$robotrange[i].length-1].split(",") if $laserscan==1
#    
#  robotx=(finalpos[3].to_f)
#  roboty=(finalpos[4].to_f)
#  
#  #draw each ray from the front sonar
#  if($drawfs==1)
#  $fsonarpos.each_with_index { |pos,index| 
#    dis=finalfson[index+3].to_f
#    #dis=1 if dis <0.8
#    sonarx=dis*( Math.cos( (finalpos[8].to_f+de2rad(pos)) ))
#    sonary=dis*( Math.sin( (finalpos[8].to_f+de2rad(pos)) ))
#   
#    glBegin(GL_LINES)
#      glVertex3f((robotx)/$scale ,(roboty)/$scale,0.0)
#      glVertex3f(( (robotx) /$scale)+(sonarx/$scale),( (roboty)/$scale)+(sonary/$scale),0.0 )
#    glEnd  
#     
#  }
#  end
#    
#  #draw each ray from the rear sonar
#  glColor(0.5, 0.0, 0.0)
#    if($drawrs==1)
#  $rsonarpos.each_with_index { |pos,index| 
#    dis=finalrson[index+3].to_f
#    #dis=1 if dis <0.8
#    sonarx=dis*( Math.cos( (finalpos[8].to_f+de2rad(pos)) ))
#    sonary=dis*( Math.sin( (finalpos[8].to_f+de2rad(pos)) ))
#    glBegin(GL_LINES)
#      glVertex3f((robotx)/$scale ,(roboty)/$scale,0.0)
#      glVertex3f((robotx/$scale)+(sonarx/$scale),(roboty/$scale)+(sonary/$scale),0.0 )
#    glEnd  
#  }
#   end
#      
#  if($laserscan==1 and $showlaserray==1)  
#  #draw the ray from the range scanner 
#  glColor(0.3,0.0,0.0)
#   finalrange.each_with_index { |member,index| 
#   # puts index
#    dis=finalrange[index].to_f
#    #dis=1 if dis <0.8
#    rangex=dis*( Math.cos( (finalpos[8].to_f-(3.1415/2)+0.0174*(180-index) ) ))
#    rangey=dis*(Math.sin( (finalpos[8].to_f-(3.1415/2)+0.0174*(180-index)  ) ))
#    glBegin(GL_LINES)
#      glVertex3f((robotx)/$scale ,(roboty)/$scale,0.0)
#      glVertex3f((robotx/$scale)+(rangex/$scale),(roboty/$scale)+(rangey/$scale),0.0 )
#    glEnd 
#  }
#  
#     # each with index
#  end
#
#
#  glPopMatrix()
#  }
#  
#
#  
#  #draw the readings from the Sensors
# 
#  $robotname.length.times{|i|
#  glPushMatrix()
#  glColor(0.0, 1.0/(i+1), 1.0/(i+1))
#  glTranslatef(0.0, 0.0, -6.0)   #Start center of the screen with a depth of six
#  #drawfsonar($robotinsarray[i],$robotfsonar[i])   
#  #drawrsonar($robotinsarray[i],$robotrsonar[i])   
#  drawrangescan($robotinsarray[i],$robotrange[i])  if($laserscan==1)
#  glPopMatrix()
#  }
 
    
#end # don't draw if can't find last data
end

# This functions tells opengl what model to draw
def draw_gl_scene()
 
  sleep($delay)
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) #clear the color & depth buffer (clear screen)
  #resets the model,destroying any old models
  glMatrixMode(GL_MODELVIEW)
  glLoadIdentity
  #gluLookAt(eye,point,up) Picture drawn on z axis -6 viewed from -12
  gluLookAt($look_x,$look_y, -12.0, $look_x, $look_y,-6.0, 0.0, 1.0, 0.0);
  #log the data into the correct format 
  
  #clear old data 
  $inslogarray=[]
  $fsonar=[]
  $rsonar=[]
  $robotname=[]
  
  #team robots
  $robotinsarray=[]
  $robotfsonar=[]
  $robotrsonar=[] 
  $robotrange=[]
  $start_x=[]
  $start_y=[]
  robotid=0
  
  
 #log from file  (read once)
  open($logfilename,"r") do |file|
   if (file)
   file.each_with_index {  |line,index|
   temparray=line.split(",") #reads the first line
   #check the robot name and match with the id position
   robotname=temparray[2]
   #match with the robotname's memory 
   if($mem_robotname.size==0)
     $mem_robotname<<robotname
     $mem_robotins=$mem_robotins<<Array.new
     $robotinsarray=$robotinsarray <<Array.new
     $mem_wall=$mem_wall<<Array.new
     robotid=$robotinsarray.length-1
   end   
   $mem_robotname.each_with_index { |member,index|
   if(robotname==member)
     robotid=index
     break
   end    
   if(index==$mem_robotname.length-1) #not found add to the robot memory
     $mem_robotname<<robotname
     $robotinsarray=$robotinsarray <<Array.new
     $mem_robotins=$mem_robotins<<Array.new
     $mem_wall=$mem_wall<<Array.new
     robotid=$robotinsarray.length-1
   end
   } 
   
   #update the INS information if the 
   if(line=~/INS,/) 
   temparray=line.split(",")
   #get the start_x and start_y values for the first starting point
   $mem_start_x[robotid]=temparray[3].to_f if !$mem_start_x[robotid]
   $mem_start_y[robotid]=temparray[4].to_f if !$mem_start_y[robotid]
    # get the robot position 
    #write the robot position   
    type=temparray[0]
    time=temparray[1].to_f
    name=temparray[2].to_f
    robotx=temparray[3].to_f
    roboty=temparray[4].to_f  
    robotz=temparray[5].to_f
    yaw=temparray[6].to_f
    pitch=temparray[7].to_f
    row=temparray[8].to_f
    pos=Array.new
    pos<<type<<time<<name<<robotx<<roboty<<robotz<<yaw<<pitch<<row
    #add last position to the robot position log
    $mem_robotpos[robotid]=Array.new(pos)
    #add the INS file
    $mem_robotins[robotid][($mem_robotins[robotid].length)]=pos
   end
   
   #RANGESCAN,67.77,robot3,1.3306,1.3349,1.3379,1.3447              
  
   #store the rangescanner information into the
   if(line=~ /RANGESCAN,/i)
     
    #calculate the wall position
    temprange=line.split(",")
    if(temprange[1].to_f>0) #ignore if time== -1
      3.times{temprange.shift}
      robotx=$mem_robotpos[robotid][3].to_f
      roboty=$mem_robotpos[robotid][4].to_f
      ang=$mem_robotpos[robotid][8].to_f
      #draw wall for each
      #drawwallrange((0.0174*(temprange.size-index)),member.to_f,robotx,roboty,tempins[8].to_f-(3.1415/2) ) # no position
      temprange.each_with_index{|member,index|  
      rad=(0.0174*(temprange.size-index))
      dis=member.to_f
      sx=robotx
      sy=roboty
      robotang=ang-(3.1415/2)
      if(dis<19.9)
       drawx=(dis)*(Math.cos(rad+robotang))
       drawy= (dis)*(Math.sin(rad+robotang))
#        glBegin(GL_POINTS)
#          glVertex3f( (sx+drawx)/$scale ,(sy+drawy)/$scale ,0.0 )
#        glEnd
        wall=[]
        wall<<(sx+drawx) <<(sy+drawy)<< 0.0
        $mem_wall[robotid]<<wall
        end     
      }
    end
          
    end
#      ins=insarray
#  range=rangescanarray
#
#if($laserscan==1)
#  range.each_with_index { |line,index| 
#    temprange=line.split(",")
#    if(temprange[1].to_f>0) #ignore if time <0 
#    3.times{temprange.shift}
#
#    #for each sonar position
#    tempins=ins[index-1].split(",")  #get the xyz position 
#    robotx=tempins[3].to_f-0#(startx)
#    roboty=tempins[4].to_f-0#(starty)
#
#    #for each range
#    temprange.each_with_index { |member,index| 
#    drawwallrange((0.0174*(temprange.size-index)),member.to_f,robotx,roboty,tempins[8].to_f-(3.1415/2) ) # no position
#      } 
#    end #end time 
#  }
#end   
#          
  
     
   } #all lines
   
      
  end # if(file)
   end #open file

 #clear the old log 
 open($logfilename,"w") 
 
 # puts $mem_wall[0].size if $mem_wall[0]
  
 draw_map() if $screen==1
  
  #swap buffers for display
  glutSwapBuffers
  

    
end

# The idle function to handle 
def idle
    # Just redisplays the screen
    glutPostRedisplay
end

# Keyboard handler to exit when ESC is typed
keyboard = lambda do |key, x, y|
    case(key)
      when 27
          glutDestroyWindow(window)
          exit(0)
      when ?a  # the a key, go left
          $look_x+=0.5 if $screen==1
          
      when ?d  # the d key, go right
          $look_x-=0.5 if $screen==1
          
      when ?w # the w key, go up
          $look_y+=0.5 if $screen==1
        
      when ?s # the s key, go down
          $look_y-=0.5 if $screen==1
      when ?+ 
          $scale-=1 if $scale>1 and $screen==1
      when ?-
          $scale+=1 if $scale<20 and $screen==1
      when ?h
          $screen=2  if $screen==1
      when ?m
          $screen=1 if $screen==2
      when ?n
           if $viewcount<$robotno
         $viewcount=$viewcount+1 
        else
          puts "no more robots"
        end
      when ?p
        if $viewcount-1>0
         $viewcount=$viewcount-1 
        else
          puts "no more robots"
        end
   end
      glutPostRedisplay
end

####################################################
#           Start initiating our code here         #
####################################################


#gets the values from the configuration file
readconfig()
#sets the startxy of the first robot, used as the center for the screen
#getstartxy() 

#initiate our OpenGL
glutInit()
#Sets the display mode to be of double buffer(enables buffer swapping) with RGB mode
#and requests an alpha and depth buffer
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
#Sets the window size and start position
glutInitWindowSize($s_width, $s_height);
glutInitWindowPosition(0,0);

window=glutCreateWindow("USAR_SIM_MAPPER") #creates a top level window

#glutDisplayFunc sets thedisplay callback for the current window. When GLUT determines 
#that the normal plane for the window needs to be redisplayed, the display callback 
# the window is called. GLUT determines when the display callback should be triggered
#  based on the window's redisplay state.when displaying, the method draw_gl_scence is called  
glutDisplayFunc(method(:draw_gl_scene).to_proc) 


#What method to call when the window is reshaped
glutReshapeFunc(method(:reshape).to_proc);

#What method to call while looping
glutIdleFunc(method(:idle).to_proc);

#uses the method keyboard to check for keyboardchanges
glutKeyboardFunc(keyboard);

#initate our window
init_gl_window($s_width, $s_height)
#tells the program to loop
glutMainLoop();




